//
// Created by wenbo on 24-9-7.
//
#include "drv_mrc.h"
#include "Array_user.h"

float demagnetization_buffer[DEMAGNETIZATION_BUFFER_SIZE] = {
        -4.8,      -4.6586,    -4.441,     -4.1542,    -3.8061,    -3.4053,    -2.9611,    -2.4831,    -1.9812,
        -1.4651,   -0.9445,    -0.42879,   0.073167,   0.5531,     1.0035,     1.4176,     1.7896,     2.1147,
        2.3892,    2.6103,     2.7762,     2.8864,     2.9413,     2.942,      2.891,      2.7911,     2.6462,
        2.4607,    2.2395,     1.9881,     1.712,      1.4171,     1.1095,     0.79509,    0.47963,    0.16876,
        -0.13224,  -0.41847,   -0.68551,   -0.92945,   -1.1469,    -1.3353,    -1.4923,    -1.6165,    -1.7072,
        -1.764,    -1.7875,    -1.7785,    -1.7385,    -1.6696,    -1.5741,    -1.4549,    -1.3149,    -1.1576,
        -0.9864,   -0.8049,    -0.61673,   -0.42546,   -0.23461,   -0.047504,  0.13271,    0.30313,    0.46118,
        0.60458,   0.73141,    0.84012,    0.92955,    0.99891,    1.0478,     1.0762,     1.0846,     1.0734,
        1.0438,    0.99711,    0.93474,    0.85849,    0.77028,    0.67214,    0.56622,    0.4547,     0.33979,
        0.22364,   0.10835,    -0.0040912, -0.11181,   -0.21311,   -0.30648,   -0.39059,   -0.46434,   -0.52688,
        -0.57755,  -0.61596,   -0.64193,   -0.65553,   -0.65702,   -0.64688,   -0.62577,   -0.59451,   -0.55407,
        -0.50555,  -0.45013,   -0.38907,   -0.32368,   -0.2553,    -0.18524,   -0.11481,   -0.045275,  0.022187,
        0.086472,  0.14658,    0.20162,    0.25084,    0.29361,    0.32944,    0.35799,    0.37905,    0.39257,
        0.39862,   0.3974,     0.38923,    0.37455,    0.35387,    0.32782,    0.29707,    0.26236,    0.22447,
        0.18419,   0.14232,    0.099685,   0.057051,   0.015175,   -0.025239,  -0.063539,  -0.099138,  -0.13152,
        -0.16025,  -0.18497,   -0.20541,   -0.22139,   -0.23281,   -0.23966,   -0.24201,   -0.24,      -0.23384,
        -0.22382,  -0.21028,   -0.19359,   -0.17418,   -0.1525,    -0.12904,   -0.10427,   -0.078688,  -0.052779,
        -0.02701,  -0.00183,   0.022342,   0.045122,   0.066166,   0.085176,   0.1019,     0.11614,    0.12775,
        0.13662,   0.14273,    0.14607,    0.14669,    0.14471,    0.14027,    0.13353,    0.12473,    0.11409,
        0.10188,   0.08838,    0.073883,   0.058683,   0.043078,   0.02736,    0.011808,   -0.0033087, -0.017743,
        -0.031268, -0.043684,  -0.054818,  -0.064527,  -0.072698,  -0.079252,  -0.084139,  -0.087342,  -0.088876,
        -0.08878,  -0.087126,  -0.084005,  -0.079534,  -0.073848,  -0.067095,  -0.05944,   -0.051055,  -0.042117,
        -0.032806, -0.023303,  -0.013782,  -0.0044108, 0.0046505,  0.013256,   0.021272,   0.028582,   0.035087,
        0.040706,  0.045375,   0.049053,   0.051715,   0.053356,   0.05399,    0.053647,   0.052373,   0.05023,
        0.047291,  0.04364,    0.039372,   0.034587,   0.02939,    0.023891,   0.018198,   0.012419,   0.0066609,
        0.0010231, -0.0043997, -0.0095211, -0.014263,  -0.018558,  -0.022349,  -0.02559,   -0.028246,  -0.030295,
        -0.031726, -0.032539,  -0.032746,  -0.032367,  -0.031434,  -0.029986,  -0.02807,   -0.025739,  -0.02305,
        -0.020067, -0.016854,  -0.013477,  -0.010001,  -0.0064937, -0.0030166, 0.00037004, 0.0036102,  0.006653,
        0.0094529, 0.011971,   0.014174};
float magnetization_buffer[MAGNETIZATION_BUFFER_SIZE] = {
        0,      0.088379, 0.22438, 0.40364, 0.62122, 0.8717, 1.1493, 1.448,  1.7618, 2.0843, 2.4097, 2.732,  3.0457,
        3.3457, 3.6272,   3.886,   4.1185,  4.3217,  4.4932, 4.6314, 4.7351, 4.804,  4.8383, 4.8388, 4.8069, 4.7444,
        4.6539, 4.5379,   4.3997,  4.2425,  4.07,    3.8857, 3.6935, 3.4969, 3.2998, 3.1055, 2.9174, 2.7385, 2.5716,
        2.4191, 2.2832,   2.1655,  2.0673,  1.9897,  1.933,  1.8975, 1.8828, 1.8885, 1.9134, 1.9565, 2.0162, 2.0907,
        2.1782, 2.2765,   2.3835,  2.4969,  2.6145,  2.7341, 2.8534, 2.9703, 3.0829, 3.1895, 3.2882, 3.3779, 3.4571,
        3.5251, 3.581,    3.6243,  3.6549,  3.6727,  3.6778, 3.6709, 3.6524, 3.6232, 3.5842, 3.5366, 3.4814, 3.4201,
        3.3539, 3.2842,   3.2124,  3.1398,  3.0677,  2.9974, 2.9301, 2.8668, 2.8085, 2.7559, 2.7098, 2.6707, 2.639,
        2.615,  2.5988,   2.5903,  2.5894,  2.5957,  2.6089, 2.6284, 2.6537, 2.684,  2.7187, 2.7568, 2.7977, 2.8404,
        2.8842, 2.9282,   2.9717,  3.0139,  3.054,   3.0916, 3.126,  3.1568, 3.1835, 3.2059, 3.2237, 3.2369, 3.2454,
        3.2491, 3.2484,   3.2433,  3.2341,  3.2212,  3.2049, 3.1857, 3.164,  3.1403, 3.1151, 3.089,  3.0623, 3.0357,
        3.0095, 2.9842,   2.9603,  2.938,   2.9178,  2.8998, 2.8844, 2.8716, 2.8616, 2.8545, 2.8502, 2.8487, 2.85,
        2.8538, 2.8601,   2.8686,  2.879,   2.8911,  2.9047, 2.9194, 2.9348, 2.9508, 2.967,  2.9831, 2.9989, 3.014,
        3.0282, 3.0414,   3.0532,  3.0637,  3.0726,  3.0798, 3.0854, 3.0892, 3.0913, 3.0917, 3.0904, 3.0877, 3.0835,
        3.078,  3.0713,   3.0637,  3.0552,  3.0462,  3.0367, 3.0269, 3.0171, 3.0074, 2.9979, 2.9889, 2.9805, 2.9727,
        2.9657, 2.9597,   2.9546,  2.9505,  2.9474,  2.9454, 2.9445, 2.9445, 2.9455, 2.9475, 2.9503, 2.9538, 2.9581,
        2.9628, 2.9681,   2.9737,  2.9795,  2.9854,  2.9914, 2.9972, 3.0029, 3.0083, 3.0133, 3.0179, 3.0219, 3.0254,
        3.0284, 3.0307,   3.0323,  3.0333,  3.0337,  3.0335, 3.0327, 3.0314, 3.0296, 3.0273, 3.0246, 3.0216, 3.0184,
        3.0149, 3.0114,   3.0078,  3.0042,  3.0006,  2.9973, 2.994,  2.9911, 2.9884, 2.986,  2.984,  2.9823, 2.9811,
        2.9802, 2.9797,   2.9795,  2.9798,  2.9804,  2.9813, 2.9825, 2.9839, 2.9856, 2.9875, 2.9895, 2.9916, 2.9937,
        2.9959, 2.9981,   3.0002,  3.0023,  3.0042,  3.0059, 3.0075, 3.0089

};

extern uint16_t ADC_BUFFER[3];

// MRC_device MRC1;a
// MRC_device MRC2;

uint16_t prescaler = 9 - 1; // Set the prescaler
uint64_t tim_clk_freq = 240000000; // Set the timer clock frequency
volatile const float mrc_supply_voltage_scale_index = 2.138/3.0;

#define MRC_PWM_FREQ 80000
#define MRC_MAX_VOLTAGE 5.0f
#define ADC_OVER_SAMPLING_RATIO 16


/**
 * @brief Initialize the MRC device.
 *
 * This function initializes the MRC device by setting its device name, input channels, timer handle, and logging
 * function. It also starts the PWM for the given timer and channels and sets the PWM parameters to 0.
 *
 * @param dev_name A pointer to the device name.
 * @param MRC A pointer to the MRC device structure.
 * @param htim A pointer to the timer handle.
 * @param IN1 The first input channel.
 * @param IN2 The second input channel.
 */
#ifdef AT8236
__WEAK void MRC_Init(const uint8_t *dev_name, MRC_device *MRC, TIM_HandleTypeDef *htim, uint32_t IN1, uint32_t IN2) {
    MRC->device_name = dev_name;
    MRC->IN1 = IN1;
    MRC->IN2 = IN2;
    MRC->pwm_handle = htim;
    MRC->LOG_MSG = printf;
    HAL_TIM_PWM_Start(htim, IN1);
    HAL_TIM_PWM_Start(htim, IN2);
    MRC_set_pwm_param(htim, IN1, MRC_PWM_FREQ, 0);
    MRC_set_pwm_param(htim, IN1, MRC_PWM_FREQ, 0);
    printf("device MRC(%s) init success!\n", dev_name);
}
#endif

#ifdef VNH7040


/**
 * @brief Initialize the MRC device
 *
 * This function initializes the MRC device, including setting the device name, timer handle, PWM channel, INA and INB
 * pins, etc.
 *
 * @param dev_name Device name
 * @param MRC Pointer to the MRC device structure
 * @param htim Timer handle
 * @param INA GPIO port for INA pin
 * @param INA_PIN GPIO pin number for INA pin
 * @param INB GPIO port for INB pin
 * @param INB_PIN GPIO pin number for INB pin
 * @param PWM PWM channel
 *
 * @details
 * The function first assigns the passed parameters to the MRC device structure, then starts the PWM channel of the
 * timer, sets the PWM parameters, and enables the INA and INB pins. Next, the function starts ADC calibration and DMA
 * transfer, and finally prints a message indicating successful device initialization.
 *
 * @note
 * This function assumes that the ADC_BUFFER array has been defined and contains two ADC values.
 *
 * @code
 * void MRC_Init(const uint8_t *dev_name, MRC_device *MRC, TIM_HandleTypeDef *htim,
 *              GPIO_TypeDef * INA, uint16_t INA_PIN,
 *              GPIO_TypeDef * INB,  uint16_t INB_PIN,
 *              uint32_t PWM)
 * {
 *     MRC->device_name = dev_name;
 *     MRC->pwm_handle = htim;
 *     MRC->PWM = PWM;                                         //PWM channel
 *     MRC->INA = INA;                                         //INA GPIO
 *     MRC->INA_PIN = INA_PIN;                                 //INA PIN
 *
 *     MRC->INB = INB;                                         //INB GPIO
 *     MRC->INB_PIN = INB_PIN;                                 //INB PIN
 *
 *     MRC->LOG_MSG = printf;
 *     HAL_TIM_PWM_Start(htim, PWM);
 *     MRC_set_pwm_param(htim, PWM, MRC_PWM_FREQ, 0);
 *     HAL_GPIO_WritePin(INA,INA_PIN,GPIO_PIN_SET);
 *     HAL_GPIO_WritePin(INB,INB_PIN,GPIO_PIN_RESET);
 *
 *     HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET_LINEARITY,ADC_SINGLE_ENDED);
 *     HAL_ADC_Start_DMA(&hadc1,(uint32_t *)&ADC_BUFFER,2);
 *
 *     printf("device MRC(%s) init success!\n", dev_name);
 * }
 * @endcode
 */
void MRC_Init(uint8_t const *dev_name, MRC_device *MRC, TIM_HandleTypeDef *htim, GPIO_TypeDef *INA, uint16_t INA_PIN,
              GPIO_TypeDef *INB, uint16_t INB_PIN, uint32_t PWM) {

    MRC->device_name = dev_name;
    MRC->pwm_handle = htim; // PWM timer handle
    MRC->PWM = PWM; // PWM channel
    MRC->INA = INA; // INA GPIO
    MRC->INA_PIN = INA_PIN; // INA PIN

    MRC->INB = INB; // INB GPIO
    MRC->INB_PIN = INB_PIN; // INB PIN

    MRC->state = Disengagement;

    MRC->LOG_MSG = printf;
    HAL_TIM_PWM_Start(htim, PWM);
    MRC_set_pwm_param(htim, PWM, MRC_PWM_FREQ, 0);
    /* OUTA enable OUTB disable*/
    HAL_GPIO_WritePin(INA, INA_PIN, GPIO_PIN_SET);
    HAL_GPIO_WritePin(INB, INB_PIN, GPIO_PIN_RESET);

    // MRC->xCoilCrtlSemaphore = xSemaphoreCreateBinary(); //create semaphore

#ifdef STM32H7
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED);

#endif

#ifdef STM32F1
    HAL_ADCEx_Calibration_Start(&hadc1);
#endif

    scaleArray(demagnetization_buffer, DEMAGNETIZATION_BUFFER_SIZE, mrc_supply_voltage_scale_index);
    scaleArray(magnetization_buffer, MAGNETIZATION_BUFFER_SIZE, mrc_supply_voltage_scale_index);

    printf("device MRC(%s) init success!\n", dev_name);
}


/**
 * @brief Process the ADC values for VNH7070_Multisense_ADC
 *
 * This function converts the values in the ADC_BUFFER array to actual voltage values and stores them in the MRC1 and
 * MRC2 structures.
 *
 * @details
 * The function calculates the actual voltage value by multiplying the value in ADC_BUFFER by 3.3 and dividing by 4095,
 * and then stores the result in the real_voltage member of MRC1 and MRC2.
 *
 * @note
 * The ADC_BUFFER array should contain two ADC values, corresponding to the voltage of MRC1 and MRC2.
 *
 * @code
 * void VNH7070_Multisense_ADC_process(void)
 * {
 *     MRC1.real_voltage = ADC_BUFFER[0] * 3.3 / 4095;
 *     MRC2.real_voltage = ADC_BUFFER[1] * 3.3 / 4095;
 * }
 * @endcode
 */
void VNH7070_Multisense_ADC_process(MRC_device *MRC1, MRC_device *MRC2) {
    MRC1->real_voltage = ADC_BUFFER[0] * 3.3 / (4095 * ADC_OVER_SAMPLING_RATIO);
    MRC2->real_voltage = ADC_BUFFER[1] * 3.3 / (4095 * ADC_OVER_SAMPLING_RATIO);
}


#endif

/**
 * @brief Set PWM parameters.
 *
 * This function sets the PWM parameters for a given timer and channel. It calculates the prescaler, timer clock
 * frequency, PWM frequency, and PWM duty cycle pulse based on the input parameters. It then sets the timer prescaler,
 * auto-reload value, and compare value accordingly.
 *
 * @param htim A pointer to the timer handle.
 * @param Channel The channel for which the PWM parameters are to be set.
 * @param freq The desired PWM frequency.
 * @param duty The desired PWM duty cycle.
 */
void MRC_set_pwm_param(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t freq, uint16_t duty) {
    float pwm_freq_arr = (tim_clk_freq * 1.0) / (prescaler + 1) / freq * 1.0 - 1; // Calculate PWM frequency
    float pwm_duty_pulse = duty * 1.0 / 100 * (pwm_freq_arr + 1); // Calculate PWM duty cycle pulse

    __HAL_TIM_SET_PRESCALER(htim, prescaler); // Set the timer prescaler
    __HAL_TIM_SetAutoreload(htim, (uint16_t) pwm_freq_arr); // Set the timer auto-reload value
    __HAL_TIM_SetCompare(htim, Channel, (uint16_t) pwm_duty_pulse); // Set the timer compare value

    // Print PWM frequency and duty cycle pulse
    // printf("pwm_freq_arr:%.2f\r\n", pwm_freq_arr);
    // printf("pwm_duty_pulse:%.2f\r\n", pwm_duty_pulse);
}

/**
 * @brief Set the voltage of the MRC device.
 *
 * This function sets the voltage of the MRC device. It calculates the duty cycle based on the input voltage and sets
 * the PWM parameters accordingly. If the voltage is greater than 5.0V, it logs a message indicating that the voltage is
 * too high.
 *
 * @param voltage The voltage to be set for the MRC device.
 * @param MRC A pointer to the MRC device structure.
 */
void MRC_set_voltage(float voltage, MRC_device *MRC) {
    uint16_t duty;

    if (voltage <= MRC_MAX_VOLTAGE) {
        MRC->voltage = voltage;

        if (voltage >= 0.0f) {
            duty = voltage / 5 * 100;
#ifdef AT8236
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN1, MRC_PWM_FREQ, duty);
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN2, MRC_PWM_FREQ, 100);
            // MRC->LOG_MSG("MRC_set_voltage: %.2f\r\n", voltage);
#endif

#ifdef VNH7040
            MRC_set_pwm_param(MRC->pwm_handle, MRC->PWM, MRC_PWM_FREQ, duty);
            /* OUTA enable OUTB disable*/
            HAL_GPIO_WritePin(MRC->INA, MRC->INA_PIN, GPIO_PIN_SET);
            HAL_GPIO_WritePin(MRC->INB, MRC->INB_PIN, GPIO_PIN_RESET);

#endif
        } else if (voltage < 0.0f) {
            duty = -voltage / 5 * 100;

#ifdef AT8236
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN1, MRC_PWM_FREQ, 100);
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN2, MRC_PWM_FREQ, duty);
// MRC->LOG_MSG("MRC_set_voltage: %.2f\r\n", voltage);
#endif

#ifdef VNH7040

            MRC_set_pwm_param(MRC->pwm_handle, MRC->PWM, MRC_PWM_FREQ, duty);
            /* OUTA disable OUTB enable*/
            HAL_GPIO_WritePin(MRC->INA, MRC->INA_PIN, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(MRC->INB, MRC->INB_PIN, GPIO_PIN_SET);

#endif
        }
    } else {
        MRC->LOG_MSG("MRC_set_voltage: The setted voltage is too high: %.2f\r\n", voltage);
    }
}

/**
 * @brief Lock the MRC device.
 *
 * This function locks the MRC device by setting its state to Magnetization and starting the timer interrupt.
 *
 * @param MRC A pointer to the MRC device structure.
 */
void MRC_lock(MRC_device *MRC) {
    MRC->state = Magnetization;
    HAL_TIM_Base_Start_IT(&htim4);
}

/**
 * @brief Unlock the MRC device.
 *
 * This function unlocks the MRC device by setting its state to Demagnetization and starting the timer interrupt.
 *
 * @param MRC A pointer to the MRC device structure.
 */
void MRC_unlock(MRC_device *MRC) {
    MRC->state = Demagnetization;
    HAL_TIM_Base_Start_IT(&htim4);
}

void MRC_switch(MRC_device *MRC, TIM_HandleTypeDef *htim) {
    if (MRC->state == Demagnetization) {
        MRC_set_voltage(demagnetization_buffer[MRC->demagnetization_counter], MRC);
        // printf("Magnetization voltage: %f\n",demagnetization_buffer[MRC->magnetization_counter]);
        MRC->demagnetization_counter++;
        if (MRC->demagnetization_counter == DEMAGNETIZATION_BUFFER_SIZE) {
            MRC->demagnetization_counter = 0;
            MRC->state = Disengagement;
            HAL_TIM_Base_Stop_IT(htim);
        }
    } else if (MRC->state == Magnetization) {
        MRC_set_voltage(magnetization_buffer[MRC->magnetization_counter], MRC);
        // printf("Magnetization voltage: %f\n",demagnetization_buffer[MRC->magnetization_counter]);
        MRC->magnetization_counter++;
        if (MRC->magnetization_counter == MAGNETIZATION_BUFFER_SIZE) {
            MRC->magnetization_counter = 0;
            MRC->state = Engagement;
            HAL_TIM_Base_Stop_IT(htim);
        }
    }
}
